#ifndef __MAP_UTILS_H__
#define __MAP_UTILS_H__

#include <nav_msgs/OccupancyGrid.h>

#include <iostream>
#include <vector>
using namespace std;
// #include <opencv2/opencv.hpp>
// using namespace cv;

#include "pose_2d.h"
#include "pixel_2d.h"

// 定义地图数据
struct Map2D{
    vector<int> grid_map;      //栅格地图
    // cv::Mat grid_map;
    int width = 0;
    int height = 0;
    float resolution = 0.05f;
    Pose2D origin_pose;   //原点位姿

    // 设置地图数据
    void SetMap(const nav_msgs::OccupancyGrid& map){ 
        this->grid_map.resize(map.data.size());
        for(size_t i = 0; i < map.data.size(); ++i){
            this->grid_map[i] = static_cast<int>(map.data[i]);
        }
        // cv::Mat map_image(map.info.height, map.info.width, CV_8UC1); 
        // for (int i = 0; i < map.data.size(); ++i) {
        //     uchar value = static_cast<uchar>(map.data[i]);
        //     if(value == 0){
        //         map_image.data[i] = 255;
        //     }
        //     else if(value == 100){
        //         map_image.data[i] = 0;
        //     }
        //     else
        //         // map_image.data[i] = 128;
        //         map_image.data[i] = 0;

        // }
        // cv::flip(map_image, grid_map, 0);    //上下翻转  PNG与ROS一致

        this->resolution = resolution;
        this->origin_pose.x = map.info.origin.position.x;
        this->origin_pose.y = map.info.origin.position.y;
        this->width = map.info.width;
        this->height = map.info.height;
    }

    // 二维数据转一维
    size_t ToIndex(Pixel2D pixel_2d){
        return pixel_2d.y * width + pixel_2d.x;
    }
    size_t ToIndex(int x, int y){
        return y * width + x;
    }

    // 一维转二维
    Pixel2D FromIndex(size_t idx){
        Pixel2D pixel_2d;
        pixel_2d.y = idx / width;
        // pixel_2d.x = idx % width;
        pixel_2d.x = idx - (pixel_2d.y * width);
        return pixel_2d;
    }

    // 真实坐标转像素坐标
    Pixel2D Pose2DToPixel2D(Pose2D pose_2d){
        Pixel2D pixel_2d;
        pixel_2d.x = (int)((pose_2d.x - origin_pose.x) / resolution);
        pixel_2d.y = (int)((pose_2d.y - origin_pose.y) / resolution);
        return pixel_2d;
    }

    // 像素坐标转真实坐标, 0.5*resolution为像素中心点位姿
    Pose2D Pixel2DToPose2D(Pixel2D pixel_2d){
        Pose2D pose_2d;
        pose_2d.x = origin_pose.x + (pixel_2d.x + 0.5) * resolution;
        pose_2d.y = origin_pose.y + (pixel_2d.y + 0.5) * resolution;
        return pose_2d;
    }

    // 检测是否有效
    bool IsValid(Pose2D pose_2d){
        if(pose_2d.x < origin_pose.x || pose_2d.y < origin_pose.y)
            return false;
        return true;
    }

    bool IsValid(Pixel2D pixel_2d){
        if(pixel_2d.x < 0 || pixel_2d.x >= width)
            return false;
        if(pixel_2d.y < 0 || pixel_2d.y >= height)
            return false;
        return true;
    }

    bool IsValid(size_t idx){
        if(idx < 0 || idx >= grid_map.size())
            return false;
        return true;
    }

    // 获取像素值，包含边界检测
    bool GetValue(Pixel2D pixel_2d, int& value){
        size_t idx = ToIndex(pixel_2d);
        if(IsValid(idx)){
            value = grid_map[idx];
            return true;
        }
        return false;
    }

    // 数值是否有效
    bool ValueIsValid(Pixel2D pixel_2d){
        int value;
        if(GetValue(pixel_2d, value)){
            if(value > 70 || value < 0) 
                return false;
            else
                return true;
        }
        return false;
    }

};





#endif
